#include "motor.h"

void	InitStepMotorTimer(TIM_TypeDef* TIMx, U8 irq)
{
	TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	NVIC_InitTypeDef NVIC_InitStructure;

	TIM_TimeBaseStructure.TIM_Prescaler = 5;
	TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1; 
	TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
	TIM_TimeBaseStructure.TIM_RepetitionCounter = 0; 
	TIM_TimeBaseInit(TIMx, &TIM_TimeBaseStructure);
 
	TIMx->SR = (uint16_t)~TIM_IT_Update;  //??TIMx???????:TIM ??? 	 
	TIM_ITConfig(TIMx,TIM_IT_Update,ENABLE ); 

	NVIC_InitStructure.NVIC_IRQChannel = irq; 
//	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1; 
	NVIC_InitStructure.NVIC_IRQChannelPriority = 6;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_Init(&NVIC_InitStructure);
	TIM_Cmd(TIMx, DISABLE);

}

void	InitStepMotor1Timer(void)
{
	InitStepMotorTimer(StepMotor1_TIMER,StepMotor1_IRQ);
}

void	InitStepMotor2Timer(void)
{
	InitStepMotorTimer(StepMotor2_TIMER,StepMotor2_IRQ);
}

static void StepMotor2PowerStop( void )
{
	GPIO_SetBits(GPIO_STEPMOTOR,GPIO_STEPM2_EN_PIN);
}

static void StepMotor2PowerStart( void )
{
	GPIO_ResetBits(GPIO_STEPMOTOR,GPIO_STEPM2_EN_PIN);
	if(StepMotor2.Dir)
			GPIO_SetBits(GPIO_STEPMOTOR,GPIO_STEPM2_DIR_PIN);
	else
			GPIO_ResetBits(GPIO_STEPMOTOR,GPIO_STEPM2_DIR_PIN);
}

static void StepMotor2TimerSet( U16 Time )
{
	StepMotor2_TIMER->ARR = Time - 1;
}

static void StepMotor2TimerStart( U16 Time )
{
	StepMotor2_TIMER->ARR = Time - 1;
	StepMotor2_TIMER->SR = (uint16_t)~TIM_IT_Update;
	TIM_Cmd(StepMotor2_TIMER, ENABLE);
}

extern   void StepMotor2TimerStop( void )
{
	TIM_Cmd(StepMotor2_TIMER, DISABLE);
	StepMotor2_TIMER->SR = (uint16_t)~TIM_IT_Update;
}

static void StepMotor2OutPutClock(void)
{
	GPIO_ResetBits(GPIO_STEPMOTOR,GPIO_STEPM2_CLK_PIN);
	StepMotor2.IntFunc = StepMotor2.IntFunc2;
}

void StepMotor2Stop( void )
{
	StepMotor2TimerStop();
	StepMotor2PowerStop();
	StepMotor2.Busy = FALSE;
	StepMotor2.Inhibit = FALSE;			
}

static void StepMotor2Poff( void )
{
	StepMotor2TimerStop();
	StepMotor2.IntFunc = StepMotor2Stop;	
	StepMotor2TimerStart(STEPMOTORPOWEROFF);
}

void StepMotor2Driver( void )
{
	bool Mount;

	GPIO_SetBits(GPIO_STEPMOTOR,GPIO_STEPM2_CLK_PIN);
	StepMotor2.IntFunc = StepMotor2OutPutClock;
	StepMotor2.Count--;
	if (StepMotor2.Count % 4)
		return;
	Get_Sensor();
	Mount = SensorNow.BIT.Seal;
	if (StepMotor2.Motion != SEAL_CTRL_MOVE)
	{
		if (StepMotor2.Mounted != Mount)
		{
			StepMotor2.Mounted = Mount;
			if (StepMotor2.Motion == SEAL_MOVE)
			{
				if (Mount)
				{
					StepMotor2.PMCounter = FD_COUNTER;;
				}
				else
				{
					StepMotor2.PMCounter = 0;
				}
			}
			else
			{
				if (Mount == FALSE)
				{
					StepMotor2.PMCounter = FD_COUNTER;
					if (StepMotor2.PMCounter > StepMotor2.Count)
						StepMotor2.PMCounter = StepMotor2.Count;
				}
				else
				{
					StepMotor2.PMCounter = 0;
				}
			}
		}
		else
		{
			if (StepMotor2.PMCounter > 0)
			{
				StepMotor2.PMCounter --;
				if (StepMotor2.PMCounter == 0)
				{
					if (StepMotor2.Motion == SEAL_MOVE)
					{
						StepMotor2.Count = SEAL_HOME_MOVE_LEN;
						StepMotor2.Motion = SEAL_HOME_MOVE;
					}
					else if (StepMotor2.Motion == SEAL_HOME_MOVE)
					{
						StepMotor2.IntFunc2 = StepMotor2Poff;							
					}
				}
			}
		}
	}
	return;
}

static void StepMotor2Break( void )
{
	StepMotor2Driver();
	if( (--StepMotor2.BrakeDist) == 0 )
	{
		StepMotor2.IntFunc2 = StepMotor2Poff;			
	}
	StepMotor2TimerSet( *StepMotor2.BrakeTable++ );
}

static void StepMotor2Move( void )
{
	StepMotor2Driver();
	if(StepMotor2.BrakeDist == StepMotor2.Count )
	{
			StepMotor2.IntFunc2 = StepMotor2Break;			
			StepMotor2.Inhibit = FALSE;			
	}
	else if( StepMotor2.BrakeDist > StepMotor2.Count )
	{
		if( StepMotor2.Count == 0 )
		{
			StepMotor2Stop();
		}
 	}
}

static void StepMotor2Accel( void )
{
	StepMotor2Driver();
	if(( (--StepMotor2.AccelDist) == 0 )||(StepMotor2.BrakeDist == StepMotor2.Count ))
	{
		if( StepMotor2.BrakeDist == StepMotor2.Count )
		{
			StepMotor2.IntFunc2  = StepMotor2Break;			
		}
		else
		{
			StepMotor2.IntFunc2  = StepMotor2Move;	
		}		
	}
	StepMotor2TimerSet( *StepMotor2.AccelTable++ );
}

static void StepMotor2Start( void )
{
	U16 Accel,Brake,Brake1;

	StepMotor2Stop();

	StepMotor2.AccelTable = (U16 *)PfmAccelTbl[StepMotor2.Speed];
	StepMotor2.BrakeTable = (U16 *)PfmBrakeTbl[StepMotor2.Speed];
	Accel = *StepMotor2.AccelTable ++;			
	Brake = *StepMotor2.BrakeTable ++;		

	if( StepMotor2.Count < Accel*2 )
	{			
		Accel = StepMotor2.Count / 2;
	}
	Brake1 = StepMotor2.Count - Accel;
	if( Brake1 < Brake )
	{					
		StepMotor2.BrakeTable += Brake - Brake1;
		Brake = Brake1;
	}
	StepMotor2.AccelDist = Accel;
	StepMotor2.BrakeDist = Brake;

	StepMotor2.Busy = TRUE;     
	StepMotor2.Inhibit = TRUE;    
	StepMotor2.IntFunc2 = StepMotor2.IntFunc =  (Accel == 0)? StepMotor2Break : StepMotor2Accel;
	StepMotor2PowerStart(); 
	StepMotor2TimerStart( STEPMOTORPOWERON );    
}

static void StepMotor1PowerStop( void )
{
	GPIO_SetBits(GPIO_STEPMOTOR,GPIO_STEPM1_EN_PIN);
}

static void StepMotor1PowerStart( void )
{
	GPIO_ResetBits(GPIO_STEPMOTOR,GPIO_STEPM1_EN_PIN);
	if(StepMotor1.Dir)
			GPIO_SetBits(GPIO_STEPMOTOR,GPIO_STEPM1_DIR_PIN);
	else
			GPIO_ResetBits(GPIO_STEPMOTOR,GPIO_STEPM1_DIR_PIN);
}

static void StepMotor1TimerSet( U16 Time )
{
	StepMotor1_TIMER->ARR = Time - 1;
}

static void StepMotor1TimerStart( U16 Time )
{
	StepMotor1_TIMER->ARR = Time - 1;
	StepMotor1_TIMER->SR = (uint16_t)~TIM_IT_Update;
	TIM_Cmd(StepMotor1_TIMER, ENABLE);
}

extern   void StepMotor1TimerStop( void )
{
	TIM_Cmd(StepMotor1_TIMER, DISABLE);
	StepMotor1_TIMER->SR = (uint16_t)~TIM_IT_Update;
}

static void StepMotor1OutPutClock(void)
{
	GPIO_ResetBits(GPIO_STEPMOTOR,GPIO_STEPM1_CLK_PIN);
	StepMotor1.IntFunc = StepMotor1.IntFunc2;
}

void StepMotor1Driver( void )
{
	bool Mount;

	GPIO_SetBits(GPIO_STEPMOTOR,GPIO_STEPM1_CLK_PIN);
	StepMotor1.Count--;
	StepMotor1.IntFunc = StepMotor1OutPutClock;
	if ((StepMotor1.Count % 4)||(StepMotor1.Motion == PF_CTRL_MOVE))
		return;
	Get_Sensor();
	Mount = !SensorNow.BIT.PaperEnd;
	if (Mount != StepMotor1.Mounted)
	{
		StepMotor1.Mounted = Mount;
		StepMotor1.PMCounter = FD_COUNTER;
		if (StepMotor1.PMCounter > StepMotor1.Count)
			StepMotor1.PMCounter = StepMotor1.Count;
	}
	else
	{
		if (StepMotor1.PMCounter > 0)
		{
			StepMotor1.PMCounter --;
			if (StepMotor1.PMCounter == 0)
			{
				if (Mount)
				{
					if (StepMotor1.Motion == PF_LOAD_MOVE)
					{
						StepMotor1.Motion = PF_SEAL_MOVE;
						StepMotor1.Count = PF_MAX_LENGTH;
					}
				}
				else
				{
					if ((StepMotor1.Motion == PF_EJECT_MOVE)||(StepMotor1.Motion == PF_SEAL_MOVE))
					{
						StepMotor1.Motion = PF_CTRL_MOVE;
						StepMotor1.Count = PF_NEAR_END_LEN;
					}
				}
			}
		}
	}
	if (Mount && (StepMotor1.Motion == PF_SEAL_MOVE))
	{
		if (StepMotor1.Seal1Pos > 0)
		{
			StepMotor1.Seal1Pos --;
			if (StepMotor1.Seal1Pos == 0)
			{
				StepMotorExec(SEAL_MOVE_LEN,SEAL_MOVE,SEAL_SLOW_SPEED,SEAL_MOTOR);
			}
		}
#if 0
		if (StepMotor1.Seal2Pos > 0)
		{
			StepMotor1.Seal2Pos --;
			if (StepMotor1.Seal2Pos == 0)
			{
			}
		}
#endif		
	}
	return;
}

void StepMotor1Stop( void )
{
	StepMotor1TimerStop();
	StepMotor1PowerStop();
	StepMotor1.Busy = FALSE;
	StepMotor1.Inhibit = FALSE;			
}

static void StepMotor1Poff( void )
{
	StepMotor1TimerStop();
	StepMotor1.IntFunc = StepMotor1Stop;	
	StepMotor1TimerStart(STEPMOTORPOWEROFF);
}
static void StepMotor1Break( void )
{
	StepMotor1Driver();
	if( (--StepMotor1.BrakeDist) == 0 )
	{
		StepMotor1.IntFunc2 = StepMotor1Poff;			
	}
	StepMotor1TimerSet( *StepMotor1.BrakeTable++ );
}

static void StepMotor1Move( void )
{
	StepMotor1Driver();
	if(StepMotor1.BrakeDist == StepMotor1.Count )
	{
			StepMotor1.IntFunc2 = StepMotor1Break;			
			StepMotor1.Inhibit = FALSE;			
	}
	else if( StepMotor1.BrakeDist > StepMotor1.Count )
	{
		if( StepMotor1.Count == 0 )
		{
			StepMotor1Stop();
		}
 	}
}

static void StepMotor1Accel( void )
{
	StepMotor1Driver();
	if(( (--StepMotor1.AccelDist) == 0 )||(StepMotor1.BrakeDist == StepMotor1.Count ))
	{
		if( StepMotor1.BrakeDist == StepMotor1.Count )
		{
			StepMotor1.IntFunc2  = StepMotor1Break;			
		}
		else
		{
			StepMotor1.IntFunc2  = StepMotor1Move;	
		}		
	}
	StepMotor1TimerSet( *StepMotor1.AccelTable++ );
}

static void StepMotor1Start( void )
{
	U16 Accel,Brake,Brake1;

	StepMotor1Stop();

	StepMotor1.AccelTable = (U16 *)PfmAccelTbl[StepMotor1.Speed];
	StepMotor1.BrakeTable = (U16 *)PfmBrakeTbl[StepMotor1.Speed];
	Accel = *StepMotor1.AccelTable ++;			
	Brake = *StepMotor1.BrakeTable ++;		

	if( StepMotor1.Count < Accel*2 )
	{			
		Accel = StepMotor1.Count / 2;
	}
	Brake1 = StepMotor1.Count - Accel;
	if( Brake1 < Brake )
	{					
		StepMotor1.BrakeTable += Brake - Brake1;
		Brake = Brake1;
	}
	StepMotor1.AccelDist = Accel;
	StepMotor1.BrakeDist = Brake;

	 StepMotor1.Busy = TRUE;     
	 StepMotor1.Inhibit = TRUE;    
	 StepMotor1.IntFunc2 = StepMotor1.IntFunc =  (Accel == 0)? StepMotor1Break : StepMotor1Accel;
	 StepMotor1PowerStart(); 
	 StepMotor1TimerStart( STEPMOTORPOWERON );    
}

static void	MakeControlBlock( short Count , U8 Motion , U8 Speed , U8 motor )
{
	STEPMOTOR stepm;
	
	Get_Sensor();	
	stepm.Motion = Motion;
	stepm.Speed = Speed;
	stepm.PMCounter = 0;
	stepm.Dir = (Count >= 0)? FALSE : TRUE;
	if(stepm.Dir )
		Count = 0 - Count;	
	stepm.Count = Count;
	if (motor == PF_MOTOR)
	{
		stepm.Seal1Pos = SealSetting.Seal1Pos;
		stepm.Seal2Pos = SealSetting.Seal2Pos;
		stepm.Mounted = !SensorNow.BIT.PaperEnd;
		StepMotor1 = stepm;
	}
	else if (motor == SEAL_MOTOR)
	{
		stepm.Mounted = SensorNow.BIT.Seal;
		StepMotor2 = stepm;
	}
}

void StepMotorExec( short ReqCount , U8 Motion , U8 Speed , U8 motor )
{
	if( ReqCount != 0 )
	{
		ReqCount <<= 2;				//Half step : 1/720 inch per steps.
		MakeControlBlock( ReqCount , Motion , Speed , motor );
		if (motor == PF_MOTOR)
			StepMotor1Start();
		else if (motor == SEAL_MOTOR)
			StepMotor2Start();
	}
}

void	WaitStepMotorStop(U8 motor)
{
	while(TRUE)
	{
		Get_Sensor();
		if (motor == PF_MOTOR)
		{
			if (StepMotor1.Busy == FALSE)
				break;
		}
		else if (motor == SEAL_MOTOR)
		{
			if (StepMotor2.Busy == FALSE)
				break;
		}
		else if (motor == 0)
		{
			if ((StepMotor1.Busy == FALSE) &&(StepMotor2.Busy == FALSE))
				break;
		}
	}
}

void StepMotorRun( short ReqCount , U8 Motion , U8 Speed , U8 motor )
{
	if(ReqCount != 0)
	{
		StepMotorExec( ReqCount, Motion , Speed, motor );
		WaitStepMotorStop(motor);
	}
}

void	Execute_Seal(void)
{
		StepMotorExec( SEAL_MOVE_LEN, SEAL_MOVE , SEAL_SLOW_SPEED, SEAL_MOTOR );	
}

void		SealHomeMove(void)
{
	Get_Sensor();
	if (SensorNow.BIT.Seal)
		StepMotorRun(SEAL_MOVE_LEN,SEAL_HOME_MOVE,SEAL_SLOW_SPEED,SEAL_MOTOR);
	Get_Sensor();
	if (SensorNow.BIT.Seal)
		ErrorCode.BIT.SealMotor = TRUE;
}

void	Check_PFMotor(void)
{
	Get_Sensor();
	if (SensorNow.BIT.PaperEnd == FALSE)
		StepMotorRun(PF_MAX_LENGTH,PF_EJECT_MOVE,PF_SLOW_SPEED,PF_MOTOR);	
}

void	ExecutePF_Seal(void)
{
	Get_Sensor();
	if (SensorNow.BIT.PaperInsert)
		StepMotorRun(PF_TRY_LOAD_LEN,PF_LOAD_MOVE,PF_SLOW_SPEED,PF_MOTOR);
	Get_Sensor();
	if (!SensorNow.BIT.PaperEnd)
		ErrorCode.BIT.PaperEnd = TRUE;
}

void TIM16_IRQHandler(void)
{
	StepMotor1_TIMER->SR = (uint16_t)~TIM_IT_Update; //TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
	(*StepMotor1.IntFunc)();
}

void TIM17_IRQHandler(void)
{
	StepMotor2_TIMER->SR = (uint16_t)~TIM_IT_Update; //TIM_ClearITPendingBit(TIM6, TIM_IT_Update);
	(*StepMotor2.IntFunc)();
}
